#include <htc.h>
#include "i2c.h"
#define SLAVE_ADD 0x1D
#define READ 0X01
#define WRITE 0X00
void i2c_start()
{
	SEN = 1;
	while(SEN);
}
void i2c_stop()
{
	PEN = 1;
	while(PEN);
}
unsigned char i2c_read(unsigned char ack)
{
	RCEN = 1;
	while(RCEN);
	ACKDT = ack;
	ACKEN = 1;
	while(ACKEN);
	return SSPBUF;
}
unsigned char i2c_write(unsigned char d)
{
	WCOL = 0;
	SSPIF = 0;
	SSPBUF = d;
	asm("nop");
	asm("nop");
	asm("nop");
	if(WCOL) return 2;			// write collision
	else {
		while(!SSPIF);
		if(ACKSTAT) return 1;	// not acknowledge
		return 0;				// everything is ok
	}
}

void i2c_init()
{
	RC3 = 1;
	RC4 = 1;
	TRISC3 = 1;
	TRISC4 = 1; //	control by mssp
	
	SSPADD = 9;		// Baudrate = Fosc/4/(SSPADDR+1)
	SMP = 1; 		// slew rate control disabled for 100KHz baudrate
	SSPCON2 = 0;	
	SSPCON = 0x28; //I2C Master Hardware mode
}

void sensor_write(unsigned char reg_addr, unsigned char data)
{
    char c;           
    i2c_start();					// Send Start condition					   
    i2c_write((SLAVE_ADD<<1)|WRITE);// Send Slave address + WRITE command 
									// ((SLAVE_ADD<<1)| WRITE) = ((0x68<<1)|0) = 0xD0   
    i2c_write(reg_addr);			// Send register address
    i2c_write(data);				// write to selected register
    i2c_stop();						// Send Stop condition
} 
char sensor_read(unsigned char reg_addr)
{ 
	char data; 
    i2c_start();					// Send Start condition    
    i2c_write((SLAVE_ADD<<1)|WRITE);// Send Slave address + WRITE command
    								// ((SLAVE_ADD<<1)| WRITE) = ((0x68<<1)|0) = 0xD0    
    i2c_write(reg_addr);			// Send register address    
    
    i2c_start();		    
    i2c_write((SLAVE_ADD<<1)|READ);	// Send Slave address + READ command
    								// ((SLAVE_ADD<<1)| READ) = ((0x68<<1)|1) = 0xD1    
    data = i2c_read(1);				// read from selected register & NACK       
    i2c_stop();						// Send Stop condition
	return data;
}

void standby_mode()
{				
	char c;
	c = sensor_read(0x2A);			// Read system control register CTRL_reg1 
	sensor_write(0x2A,c&(~0X01));	// Clear active bit
}

void active_mode()
{
	char c;
	c = sensor_read(0x2A);			// Read system control register CTRL_reg1
	sensor_write(0x2A, c | 0X01);	// Set active bit
}
